#include "LikelihoodCache.h"

LikelihoodCache::LikelihoodCache()
{
	/*
	Default constructor for the likelihood cache.  Enters default
	values to ensure that the cache is invalid.

	INPUTS:
		none.

	OUTPUTS:
		none.
	*/

	//enter default values for the synch variables
	mCacheTime = LC_DEFAULTTIME;
	mNumMeasurements = LC_DEFAULTNM;
	mSensorID = LC_DEFAULTID;
	mMetaMeasurementType = LC_DEFAULTMM;

	//enter garbage for the cached values
	nz = 0;
	nx = 0;
	zbar = NULL;
	PbarHt = NULL;
	HPbarHt = NULL;

	return;
}

LikelihoodCache::~LikelihoodCache()
{
	/*
	Destructor for the likelihood cache.  Deletes all memory
	and invalidates the cache.

	INPUTS:
		none.

	OUTPUTS:
		none.
	*/

	//clear out all the memory allocated
	ResetCache();

	return;
}

void LikelihoodCache::ResetCache()
{
	/*
	Resets all the cache values to default (deletes the cache)

	INPUTS:
		none.

	OUTPUTS:
		none.
	*/

	//enter default values for the synch variables
	mCacheTime = LC_DEFAULTTIME;
	mNumMeasurements = LC_DEFAULTNM;
	mSensorID = LC_DEFAULTID;
	mMetaMeasurementType = LC_DEFAULTMM;

	//delete the cached values
	nz = 0;
	nx = 0;
	delete [] zbar;
	zbar = NULL;
	delete [] PbarHt;
	PbarHt = NULL;
	delete [] HPbarHt;
	HPbarHt = NULL;

	return;
}

bool LikelihoodCache::IsCached(double iCurrentTime, int iSensorID, int iMetaMeasurementType, int iNumMeasurements, int inz, int inx)
{
	/*
	Determines whether the desired lieklihood parameters are cached, by
	comparing the cached timestamp, sensor, and measurement type to the
	input values.

	INPUTS:
		iCurrentTime - time of the measurement that is in question
		iSensorID - ID of the sensor used to make the query measurement
		iMetaMeasurementType - type of the MM generated by the sensor
		iNumMeasurements - number of measurements the target has received
			at the time of the cache
		inz - number of measurements (length of the vector)
		inx - number of state variables (length of the vector)

	OUTPUTS:
		rIsCached - true if the likelihood parameters are cached here, 
			false otherwise.
	*/

	bool rIsCached = false;

	if (mCacheTime == iCurrentTime && mSensorID == iSensorID &&
		mMetaMeasurementType == iMetaMeasurementType && 
		mNumMeasurements == iNumMeasurements && inz == nz && inx == nx)
	{
		//the measurement correlations are already cached
		rIsCached = true;
	}

	return rIsCached;
}

void LikelihoodCache::SetCache(double iCacheTime, int iSensorID, int iMetaMeasurementType, int iNumMeasurements, 
							   int inz, int inx, double* inu, double* iS, double* iW, double* iz, double* iR)
{
	/*
	Sets the likelihood cache after a brute force call to the full likelihood
	function is made.

	INPUTS:
		iCacheTime - time on the newly-cached measurement
		iSensorID - ID of the sensor for which the measurement is computed
		iMetaMeasurementType - type of MM just generated
		iNumMeasurements - number of measurements applied to the target so far
		inz - length of the measurement vector
		inx - length of the state vector
		inu - measurement innovation returned by the full likelihood
		iS - measurement covariance returned by the full likelihood
		iW - measurement gain matrix returned by the full likelihood
		iz - the actual measurement (not cached, but used to calculate zbar)
		iR - the measurement's covariance (not cached, but used to calculate correlations)

	OUTPUTS:
		none.  Caches input values.  NOTE: matrices are value copied, so inu, iS, iW, etc.
			should be memory-managed elsewhere.
	*/

	int i;
	int j;
	int k;
	int idx;

	//clear out any existing cache
	ResetCache();

	//copy over the identifier flags
	mCacheTime = iCacheTime;
	mSensorID = iSensorID;
	mMetaMeasurementType = iMetaMeasurementType;
	mNumMeasurements = iNumMeasurements;
	nz = inz;
	nx = inx;

	if (nz > 0)
	{
		//declare memory for the cached measurement and HPH'
		zbar = new double[nz];
		HPbarHt = new double[nz*nz];

		//calculate zbar from nu = z - zbar
		for (i = 0; i < nz; i++)
		{
			zbar[i] = iz[i] - inu[i];
		}

		//calculate HPH' = S - R
		for (i = 0; i < nz; i++)
		{
			for (j = 0; j < nz; j++)
			{
				idx = midx(i, j, nz);
				HPbarHt[idx] = iS[idx] - iR[idx];
			}
		}

		if (nx > 0)
		{
			PbarHt = new double[nx*nz];
			memset(PbarHt, 0x00, nx*nz*sizeof(double));

			//calculate PbarHt = W*S
			for (i = 0; i < nx; i++)
			{
				for (j = 0; j < nz; j++)
				{
					idx = midx(i, j, nx);
					for (k = 0; k < nz; k++)
					{
						PbarHt[idx] += iW[midx(i, k, nx)] * iS[midx(k, j, nz)];
					}
				}
			}
		}
	}

	return;
}

double LikelihoodCache::FastLikelihood(double* nu, double* S, double* W, double* z, bool* zwrap, double* R, double iChi2Gate)
{
	/*
	Computes a fast likelihood of the given measurement and covariance using the correlation and
	covariance values precomputed in the cache.

	INPUTS:
		nu, S, W - will contain the innovation, innovation covariance, and Kalman
			gain matrix computed for the target.
		z, zwrap, R - measurement, which elements are to be wrapped angles, and measurement covariance
		iChi2Gate - a chi2 gate to be applied to the measurement.

	OUTPUTS:
		rLambda - the measurement likelihood.  Also populates nu, S, and W.
			If an error is encountered, the matrices are not populated and rLambda = 0.0.

		NOTE: this assumes IsCached() has already been checked for the measurement.
	*/

	double rLambda = 0.0;

	int i;
	int j;
	int k;
	int idx;

	if (nx <= 0 || nz <= 0)
	{
		printf("Warning: LikelihoodCache::FastLikelihood called with nz = %d and nx = %d.\n", nz, nx);
		return rLambda;
	}

	//declare working memory
	double* invS = (double*) _malloca(nz*nz*sizeof(double));
	int* ipiv = (int*) _malloca(nz*sizeof(int));
	int info;

	//calculate innovation nu from z and zbar
	for (i = 0; i < nz; i++)
	{
		if (zwrap[i] == false)
		{
			nu[i] = z[i] - zbar[i];
		}
		else
		{
			nu[i] = UnwrapAngle(z[i] - zbar[i]);
		}
	}

	//calculate S = HPH' + R
	for (i = 0; i < nz; i++)
	{
		for (j = 0; j < nz; j++)
		{
			idx = midx(i, j, nz);
			S[idx] = HPbarHt[idx] + R[idx];
		}
	}

	//calculate inv(S)
	memcpy(invS, S, nz*nz*sizeof(double));
	//LU decomposition for matrix inversion
	dgetrf(nz, nz, invS, nz, ipiv, &info);
	if (info != 0)
	{
		printf("Warning: dgetrf error in LikelihoodCache::FastLikelihood.\n");

		_freea(invS);
		_freea(ipiv);

		return rLambda;
	}
	//calculate the determinant of S before destroying the LU decomposition
	double detS = 1.0;
	for (i = 0; i < nz; i++)
	{
		if (ipiv[i] > i+1)
		{
			//negate the determinant because a row pivot took place
			detS *= -invS[midx(i, i, nz)];
		}
		else
		{
			//don't negate the determinant because the ith row either wasn't pivoted
			//or it was pivoted (but we counted it already)
			detS *= invS[midx(i, i, nz)];
		}
	}
	//invert S and store in invS
	dgetri(nz, invS, nz, ipiv, &info);
	if (info != 0)
	{
		printf("Warning: dgetri error in LikelihoodCache::FastLikelihood.\n");

		_freea(invS);
		_freea(ipiv);

		return rLambda;
	}

	//calculate W = PH'*inv(S)
	memset(W, 0x00, nx*nz*sizeof(double));
	for (i = 0; i < nx; i++)
	{
		for (j = 0; j < nz; j++)
		{
			idx = midx(i, j, nx);
			for (k = 0; k < nz; k++)
			{
				W[idx] += PbarHt[midx(i, k, nx)] * invS[midx(k, j, nz)];
			}
		}
	}

	//calculate likelihood: 
	//rLambda = exp(-0.5*nu'*inv(S)*nu) / sqrt((2*pi)^(length(nu))*det(S))
	for (i = 0; i < nz; i++)
	{
		for (j = 0; j < nz; j++)
		{
			rLambda += nu[i] * invS[midx(i, j, nz)] * nu[j];
		}
	}
	if (rLambda > iChi2Gate)
	{
		//measurement failed the gating hypothesis test
		rLambda = 0.0;

		_freea(invS);
		_freea(ipiv);

		return rLambda;
	}
	//rLambda = exp(-0.5*rLambda) / sqrt(pow(TWOPI, (double) (nz))*detS);
	//do this for a little bit more stability numerically
	rLambda = exp(-0.5*rLambda - 0.5*((double) nz)*LNTWOPI - 0.5*log(detS));

	//free working memory
	_freea(invS);
	_freea(ipiv);

	return rLambda;
}
